package com.rockwell.robot2015.model.states.running.sequence;

import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotAction;
import com.rockwell.robot2015.model.states.IState;
import com.rockwell.robot2015.model.states.IStateMachine;
import com.rockwell.robot2015.model.states.running.RobotRunningStateMachine;

public class DroppingTreasureSequence extends RobotRunningSequence {
	private static final int TREASURE_CLEARANCE = 12;
	
	enum State implements IState {
		Start {

			@Override
			public void enter(IStateMachine machine) {
			}

			@Override
			public void move(IStateMachine machine) {
				machine.robot().log("Start dropping treasure", 2);
				machine.changeState(ArmDownOpen);
			}

			@Override
			public void exit(IStateMachine machine) {
			}
			
		},
		ArmDownOpen {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Arm down", 4);
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				robot.sendCommand(RobotAction.MoveToTreasure, -15);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				robot.sendCommand(RobotAction.ArmDown);
				//change armdown so that it pulses in reverse for a short time, then pulses x cycles to drop arm and open fully
				machine.changeState(Dropped);
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
			
		},
		KeepArm {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void move(IStateMachine machine) {
				machine.robot().sendCommand(RobotAction.ArmUp);
				//need to update robot commander pick()
				//send pulse
				machine.changeState(Dropped);	
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		Dropped {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Treasure dropped", 4);
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				robot.log("Move back", 4);
				robot.sendCommand(RobotAction.MoveToTreasure, -TREASURE_CLEARANCE);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				robot.log("Arm up", 4);
				robot.sendCommand(RobotAction.ArmUp);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				robot.log("Turn back", 4);
				robot.turnBack();
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				robot.log("Try to adjust angle", 4);
				robot.adjustAngle(Robot.ADJUST_ANGLE_THRESHOLD);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				machine.changeState(Done);
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		Done {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Dropping treasure done", 4);
				
				machine.robot().onTreasureDropped();
			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		};
		
		static DroppingTreasureSequence m_sequence;
		
		public static void setSequence(DroppingTreasureSequence sequence) {
			m_sequence = sequence;
		}
	}
	public DroppingTreasureSequence(RobotRunningStateMachine running) {
		super(running, State.Start);
		State.setSequence(this);
		m_currState = State.Start;
	}

	@Override
	public void start() {
		m_currState = State.Start;
		m_nextState = null;
		
		Robot robot = m_running.robot();
		robot.enableTreasureDetector(false);
		//robot.enableObstacleDetector(false);
		robot.enableMaze1Detector(false);
		robot.enableMaze2Detector(false);
		robot.enableMaze3Detector(false);
		robot.enableMazeEntranceGuideDetector(false);
		robot.enableDropAreaDetector(true);
		robot.enableOuterWallDetector(false);
	}

	@Override
	public boolean succeeded() {
		return m_currState.equals(State.Done);
	}

	@Override
	public boolean failed() {
		return false;
	}
}
